/***********************************************************
     Starter code for Assignment 3

     This code was originally written by Jack Wang for
		    CSC418, SPRING 2005

		implements scene_object.h

***********************************************************/

#include <cmath>
#include <iostream>
#include "scene_object.h"

bool UnitSquare::intersect( Ray3D& ray, const Matrix4x4& worldToModel,
		const Matrix4x4& modelToWorld ) {
	// TODO: implement intersection code for UnitSquare, which is
	// defined on the xy-plane, with vertices (0.5, 0.5, 0), 
	// (-0.5, 0.5, 0), (-0.5, -0.5, 0), (0.5, -0.5, 0), and normal
	// (0, 0, 1).
	//
	// Your goal here is to fill ray.intersection with correct values
	// should an intersection occur.  This includes intersection.point, 
	// intersection.normal, intersection.none, intersection.t_value.   
	//
	// HINT: Remember to first transform the ray into object space  
	// to simplify the intersection test.
	Point3D origin(0,0,0);
		Vector3D rayDir = worldToModel * ray.dir;
		Point3D rayOrigin = worldToModel * ray.origin;
		Vector3D squareNormal(0,0,1);
		double lambdaStar;
		Point3D intersectionPoint;

		if (rayDir[2] != 0)
		{
			lambdaStar = -rayOrigin[2]/rayDir[2];
			if ((lambdaStar*lambdaStar)< 0.00001){
				return false;
			}
		}

		//the ray and the square are on the same plane
		else if (rayDir[2] == 0 && rayOrigin[2] == 0)
		{
			double lambda_1 = (-1.5 - rayOrigin[0])/rayDir[0];
			double lambda_2 = (1.5 - rayOrigin[0])/rayDir[0];
			double lambda_3 = (-1.5 - rayOrigin[1])/rayDir[1];
			double lambda_4 = (1.5 - rayOrigin[1])/rayDir[1];
			lambdaStar = lambda_1;
			if (lambda_2 < lambdaStar)
			{
				lambdaStar = lambda_2;
			}
			if (lambda_3 < lambdaStar)
			{
				lambdaStar = lambda_3;
			}
			if (lambda_4 < lambdaStar)
			{
				lambdaStar = lambda_4;
			}
		}
		else
		{

			return false;
		}


		intersectionPoint = rayOrigin + lambdaStar * rayDir;
		if (intersectionPoint[0] < 1.5 && intersectionPoint[0] > -1.5 && intersectionPoint[1] < 1.5 && intersectionPoint[1] > -1.5)
		{
			if (!ray.intersection.none && lambdaStar > ray.intersection.t_value){
				return false;
			}
			ray.intersection.point = modelToWorld * intersectionPoint;
			ray.intersection.none = false;
			ray.intersection.t_value = lambdaStar;
			Vector3D normal(0,0,1);
			ray.intersection.normal = modelToWorld * normal;

			return true;
		}
		else
		{
			return false;
		}

//	return false;
}

bool UnitSphere::intersect( Ray3D& ray, const Matrix4x4& worldToModel,
		const Matrix4x4& modelToWorld ) {
	// TODO: implement intersection code for UnitSphere, which is centred 
	// on the origin.  
	//
	// Your goal here is to fill ray.intersection with correct values
	// should an intersection occur.  This includes intersection.point, 
	// intersection.normal, intersection.none, intersection.t_value.   
	//
	// HINT: Remember to first transform the ray into object space  
	// to simplify the intersection test.
	
	Point3D rayOrigin = worldToModel * ray.origin;

		Vector3D rayDir = worldToModel * ray.dir;
		Point3D sphereOrigin(0,0,0);
		double lambdaStar;
		//std::cout << "ray origin is " << rayOrigin << "\n";
		//std::cout << "ray direction is " << rayDir << "\n";

		double A = rayDir.dot(rayDir);
		double B = (rayOrigin-sphereOrigin).dot(rayDir);
		double C = (rayOrigin-sphereOrigin).dot(rayOrigin-sphereOrigin) - 1;
		double D = B*B-A*C;

		//std::cout << "A, B, C, D are "<< A << "," << B << "," << C << "," << D << "\n";
		if (D<0)
		{
			return false;
		}else if(D*D < 0.00001){
			double lambda = -B/A;
			if (((lambda-0)*(lambda-0)) < 0.00001){
				return false;
			}else{
				lambdaStar = lambda;
			}
		}
		else
		{
			double lambda_1 = -B/A + sqrt(D) / A;
			double lambda_2 = -B/A - sqrt(D) / A;
			//std::cout << "lambda1  "<< lambda_1 << " lambda2 " << lambda_2 << "\n";
			if (lambda_1 < 0 && lambda_2 < 0)
			{
				return false;
			}
			else if (lambda_1 > 0 && lambda_2 < 0)
			{
				lambdaStar = lambda_1;
			}
			else
			{
				lambdaStar = lambda_2;
			}
		}
		if (lambdaStar < 0.01){
			return false;
		}
		Point3D intersectionPoint = rayOrigin + lambdaStar * rayDir;
		Vector3D normal = intersectionPoint - sphereOrigin;

		if (!ray.intersection.none && lambdaStar > ray.intersection.t_value){
			return false;
		}
		ray.intersection.point = modelToWorld * intersectionPoint;
		//std::cout << "ray origin " << ray.origin << "\n";
		//std::cout << "intersection point " << ray.intersection.point << "\n";
		/*if ((ray.origin - ray.intersection.point).length() < 0.0001){
			return false;
		}*/
		ray.intersection.normal = modelToWorld * normal;
		ray.intersection.normal.normalize();
		ray.intersection.t_value = lambdaStar;
		ray.intersection.none = false;
		return true;

	//return false;
}

bool UnitCylinder::intersect( Ray3D& ray, const Matrix4x4& worldToModel,
		const Matrix4x4& modelToWorld ) {
	//NOTE: The intersection for a unit cylinder. A unit cylinder here means that
	//      the top and the base are unit circles centered at (0,0,z), where z is
	//      -0.5 and 0.5, respectively. And the height of the cylinder is 1

	Point3D rayOrigin = worldToModel * ray.origin;
	Vector3D rayDir = worldToModel * ray.dir;
	Point3D sphereOrigin(0,0,0);
	double lambdaStar_1;
	double lambdaStar_2;
	double lambdaStar;
	double lambda_1;
	double lambda_2;
	double A = rayDir[0]*rayDir[0] + rayDir[1]*rayDir[1];
	double B = (rayOrigin[0]*rayDir[0] + rayOrigin[1] * rayDir[1]);
	double C = rayOrigin[0]*rayOrigin[0] + rayOrigin[1]*rayOrigin[1] - 1;
	double D = B*B-A*C;
	Point3D intersectionPoint;
	Vector3D normal_1;
	Vector3D normal_2;
	Vector3D normal;

	//Getting the lambda for the closer base
	if (rayDir[2] != 0)
	{
		lambda_1 = (-0.5-rayOrigin[2])/rayDir[2];
		lambda_2 = (0.5-rayOrigin[2])/rayDir[2];
		if (lambda_1 < lambda_2)
		{
			lambdaStar_1 = lambda_1;
			Point3D normal_temp(0,0,-1);
			normal_1 = normal_temp - sphereOrigin;
			normal_1.normalize();

		}
		else
		{
			lambdaStar_1 = lambda_2;
			Point3D normal_temp(0,0,1);
			normal_1 = normal_temp - sphereOrigin;
			normal_1.normalize();
		}
	}

	//Getting the lambda for the side of the cylinder
	if (D<0)
	{
		return false;
	}
	else
	{
		double lambda_1 = -B/A + sqrt(D) / A;
		double lambda_2 = -B/A - sqrt(D) / A;
		if (lambda_1 < 0 && lambda_2 < 0)
		{
			return false;
		}
		else if (lambda_1 > 0 && lambda_2 < 0)
		{
			lambdaStar_2 = lambda_1;
		}
		else
		{
			lambdaStar_2 = lambda_2;
		}
	}



	intersectionPoint = rayOrigin + lambdaStar_1 * rayDir;
	if (lambdaStar_1* lambdaStar_1 < 0.0000001){
		return false;
	}
	//Use the first lambda to check if it intersects with the base (or top)
	if (intersectionPoint[0]*intersectionPoint[0] + intersectionPoint[1] * intersectionPoint[1] <= 1)
	{
		if (!ray.intersection.none && lambdaStar > ray.intersection.t_value){
			return false;
		}
		ray.intersection.point = intersectionPoint;

		ray.intersection.normal = normal_1;
		ray.intersection.t_value = lambdaStar_1;
		ray.intersection.none = false;
		return true;
	}
	else
	{
		 //if not intersected with the base, use the second lamdba to check if intersects with the side

		intersectionPoint = rayOrigin + lambdaStar_2 * rayDir;
		if (lambdaStar_2 * lambdaStar_2 < 0.00000001){
			return false;
		}
		normal_2[0] = intersectionPoint[0];
		normal_2[1] = intersectionPoint[1];
		normal_2[2] = 0;
		normal_2.normalize();



		if (intersectionPoint[2] < 0.5 && intersectionPoint[2] > -0.5)
		{
			if (!ray.intersection.none && lambdaStar > ray.intersection.t_value){
				return false;
			}
			ray.intersection.point = modelToWorld * intersectionPoint;
			Point3D normalll;
			normalll[0] = intersectionPoint[0];
			normalll[1] = intersectionPoint[1];
			normalll[2] = 0;
			ray.intersection.normal = modelToWorld * (normalll - sphereOrigin);
			ray.intersection.t_value = lambdaStar_2;
			ray.intersection.none = false;
			return true;
		}
		else
		{
			return false;
		}
	}
}


